/*
 **********************************************************************
 * File name: euler.h
 *
 * COPYRIGHT (C) 2018-2022
 *
 * The license and distribution terms for this file may be found in 
 * the LICENSE file.
 *
 * Author: ChenLong (Add new editor)
 * Email: worldlong@foxmail.com
 **********************************************************************
*/
#pragma once
/*
*/
#include <cmath>
#include <cstring>
#include <cstdint>
#include "vector.h"
#include "euler.h"
#include "dcm.h"
/*
*/
#ifndef PI
#define PI (3.14159265358979323846f)
#endif
#ifndef RAD2DEG
#define RAD2DEG (180.0f / PI)
#endif
#ifndef DEG2RAD
#define DEG2RAD (PI / 180.0f)
#endif
/*
*/
namespace matrix
{
/*
*/
template<typename Type>
class Quaternion_T;

template<typename Type>
class DCM_T;

template<typename Type>
class Vector3;

/*---------------------------------------
                   Euler
       Rotate from global to local
 *----------------------------------------*/
template<typename Type>
class Euler_T : public Vector<Type, 3>
{
public:
  
  Type &roll = this->data[0][0];
  Type &pitch = this->data[0][1];
  Type &yaw = this->data[0][2];
  /*
  */
  Euler_T() {}
  
  Euler_T(float roll0, float pitch0, float yaw0)
  {
    roll = roll0;
    pitch = pitch0;
    yaw = yaw0;
  }
  
  Euler_T(const Vector<Type, 3> &vb) :
    Vector3<Type>(vb)
  {}
  
  Euler_T(const Vector3<Type> &vb) :
    Vector3<Type>(vb)
  {}
  
  Euler_T(const Matrix<Type, 1, 3> &vb)
  {
    if (this != &vb) {
      memcpy(this->data, vb.data, sizeof(this->data));
    }
  }
  
  Euler_T(const Euler_T<Type> &eb)
  {
    roll = eb.roll;
    pitch = eb.pitch;
    yaw = eb.yaw;
  }
  
  Euler_T(const Quaternion_T<Type> &q, uint32_t type)
  {
    float tmp;
    if(type == 321)  //means rotating order is Z->Y->X
    {
      tmp = 2*(q.q1*q.q3 - q.q0*q.q2);
      tmp = tmp > 1.0f ? 1.0f : (tmp < -1.0f ? -1.0f : tmp);
      pitch = -asinf(tmp); 
      roll = atan2f(2*(q.q2*q.q3 + q.q0*q.q1), 1 - 2*(q.q1*q.q1 + q.q2*q.q2)); 
      yaw = atan2f(2*(q.q0*q.q3 + q.q1*q.q2), 1 - 2*(q.q2*q.q2 + q.q3*q.q3));
    }
    else if(type == 312) //means rotating order is Z->X->Y
    {
      tmp = 2*(q.q2*q.q3 + q.q0*q.q1);
      tmp = tmp > 1.0f ? 1.0f : (tmp < -1.0f ? -1.0f : tmp);
      roll = asinf(tmp); 
      pitch = -atan2f(2*(q.q1*q.q3 - q.q0*q.q2), 1 - 2*(q.q1*q.q1 + q.q2*q.q2)); 
      yaw = -atan2f(2*(q.q1*q.q2 - q.q0*q.q3), 1 - 2*(q.q1*q.q1 + q.q3*q.q3));
    }
  }

  Euler_T(const DCM_T<Type> &dcm, uint32_t type)
  {
    if(type == 321)  //means rotating order is Z->Y->X
    {
      pitch = -asinf(dcm.data[0][2]);
      roll = atan2f(dcm.data[1][2], dcm.data[2][2]);
      yaw = atan2f(dcm.data[0][1], dcm.data[0][0]);
    }
    else if(type == 312) //means rotating order is Z->X->Y
    {
      roll = asinf(dcm.data[1][2]);
      pitch = atan2f(-dcm.data[0][2], dcm.data[2][2]);
      yaw = atan2f(-dcm.data[1][0], dcm.data[1][1]);
    }
  }

  /*------------------------------------------*
   * Operator overload                        *
   *------------------------------------------*/
  /*
   * E1 = E2
   */
  Euler_T<Type> &operator=(const Euler_T<Type> &vb)
  {
    if (this != &vb) {
      memcpy(this->data, vb.data, sizeof(this->data));
    }
    return (*this);
  }
  
  /*
   * E1 = V2
   */
  Euler_T<Type> &operator=(const Vector3<Type> &vb)
  {
    if (this != &vb) {
      memcpy(this->data, vb.data, sizeof(this->data));
    }
    return (*this);
  }
  
  /*
   * E1 = V2
   */
  Euler_T<Type> &operator=(const Vector<Type, 3> &vb)
  {
    if (this != &vb) {
      memcpy(this->data, vb.data, sizeof(this->data));
    }
    return (*this);
  }
  
  /*
   * E1 = M2
   */
  Euler_T<Type> &operator=(const Matrix<Type, 1, 3> &vb)
  {
    if (this != &vb) {
      memcpy(this->data, vb.data, sizeof(this->data));
    }
    return (*this);
  }
  
  /*------------------------------------------*
   * Functions                                *
   *------------------------------------------*/
  inline Euler_T<Type> to_radian()
  {
    *this *= DEG2RAD;
    return (*this);
  }
  
  inline Euler_T<Type> to_degree()
  {
    *this *= RAD2DEG;
    return (*this);
  }
  
  inline Vector3<Type> rotate(const Vector3<Type> &vb, uint32_t type) const
  {
    DCM_T<Type> dcm(*this, type);
    return dcm * vb;
  }
  
  inline Vector3<Type> rotate_to_local(const Vector3<Type> &vb, uint32_t type) const
  {
    return rotate(vb, type);
  }
  
  inline Vector3<Type> rotate_to_global(const Vector3<Type> &vb, uint32_t type) const
  {
    DCM_T<Type> dcm(*this, type);
    dcm = dcm.transpose();
    return dcm * vb;
  }
  
  /*
  */
  Vector3f rate(Vector3f gyro, uint32_t type)
  {
    float sr = sinf(roll);
    float cr = cosf(roll);
    float sp = sinf(pitch);
    float cp = cosf(pitch);
    Vector3f vr;
    
    if(type == 321)
    {
      if(fabsf(cp) > 1e-3f)
      {
        vr.x = gyro.x + gyro.y*sr*sp/cp + gyro.z*cr*sp/cp;
        vr.y =          gyro.y*cr       - gyro.z*sr;
        vr.z =          gyro.y*sr/cp    + gyro.z*cr/cp;
      }
      else
      {
        vr.x  =-gyro.z;
        vr.y  = gyro.y;
        vr.z  = gyro.x;
      }
    }
    else if(type == 312)
    {
      if(fabsf(cr) > 1e-3f)
      {
        vr.x = gyro.x*cp                + gyro.z*sp;
        vr.y = gyro.x*sp*sr/cr + gyro.y - gyro.z*cp*sr/cr;
        vr.z =-gyro.x*sp/cr             + gyro.z*cp/cr;       
      }
      else
      {
        vr.x = gyro.x;
        vr.y =-gyro.z;
        vr.z = gyro.y;
      }
    }
    
    return vr;
  }
};

typedef Euler_T<float> Euler;    //default Euler is float type

}


